#include "EmergencyStopState.hpp"
#include "EventType.hpp"
#include "FreestyleState.hpp"
#include "Robot.hpp"
#include "Side.hpp"

#include "CollisionSensor.hpp"
#include "DistanceSensor.hpp"

Robot& Robot::getInstance()
{
	static FreestyleState startState;
	static Robot robot(startState);
	return robot;
}

Robot::Robot(State& aStartState) :
	currentState(aStartState),
	sensorPoller()
{

}

void Robot::handleEvent(Event& event)
{
	switch (event.getEventType())
	{
		case EMERGENCYSTOPACTIVATED:
		{
			//handle this event:
			handleEmergencyStopActivatedEvent();
			break;
		}
		default: //Everything else
		{
			currentState.handleEvent(event); //Let the current state handle this event
			break;
		}
	}
}

void Robot::handleEmergencyStopActivatedEvent()
{
//	//Check if current state is EmergencyStopState
//	if (currentState.getStateType() != State::EMERGENCYSTOP)
//	{ //Current state is not EMERGENCYSTOP
//		//create new emergency state
//		EmergencyStopState newState;
//		changeState(newState);
//	}
}

void Robot::changeState(State& newState)
{
	//Change currentState
	currentState = newState;
	//Initialize new state
	currentState.initState();
}

State& Robot::getState()
{
	return currentState;
}

Engine& Robot::getEngine()
{
	return engine;
}

void Robot::init()
{
	//Init sensors:
	//TODO: Init sensors here

	//Post check:
	//TODO: POST Check here

	//Start robot:
	currentState.initState();

	//Init sensors:
	CollisionSensor collisionSensorBack(BACK);
	sensorPoller.addSensor(collisionSensorBack);

	CollisionSensor collisionSensorFront(FRONT);
	sensorPoller.addSensor(collisionSensorFront);

	DistanceSensor distanceSensor;
	sensorPoller.addSensor(distanceSensor);

	//(Re)start robot
	restart();
}

void Robot::restart()
{
	//TODO: keep polling sensors
	while (true)
	{
		sensorPoller.poll();
//		if (currentState.getStateType() == State::FREESTYLE)
//			currentState.initState();
	}
}
